#include "cpu.h"
#include "mem.h"
#include<cstdio>

cpu::cpu(mem * m,unsigned short pc_init = 0x100) {
    this->m=m;
    total_cycles=0;
    frame_cycles=0;
    int_enable=true;
    pc=new unsigned short;
    af=new unsigned short;
    bc=new unsigned short;
    de=new unsigned short;
    hl=new unsigned short;
    sp=new unsigned short;
    a=reinterpret_cast<unsigned char *>(af+1);
    b=reinterpret_cast<unsigned char *>(bc+1);
    c=reinterpret_cast<unsigned char *>(bc);
    d=reinterpret_cast<unsigned char *>(de+1);
    e=reinterpret_cast<unsigned char *>(de);
    f=reinterpret_cast<unsigned char *>(af);
    h=reinterpret_cast<unsigned char *>(hl+1);
    l=reinterpret_cast<unsigned char *>(hl);
    *pc=pc_init;
    *af=0x11b0;
    *bc=0x0013;
    *de=0x00d8;
    *hl=0x014d;
    *sp=0xfffe;
    m->write_byte(0xff05,0);
    m->write_byte(0xff06,0);
    m->write_byte(0xff07,0);
    m->write_byte(0xff10,0x80);
    m->write_byte(0xff11,0xbf);
    m->write_byte(0xff12,0xf3);
    m->write_byte(0xff14,0xbf);
    m->write_byte(0xff16,0x3f);
    m->write_byte(0xff17,0);
    m->write_byte(0xff19,0xbf);
    m->write_byte(0xff1a,0x7f);
    m->write_byte(0xff1b,0xff);
    m->write_byte(0xff1c,0x9f);
    m->write_byte(0xff1e,0xbf);
    m->write_byte(0xff20,0xff);
    m->write_byte(0xff21,0);
    m->write_byte(0xff22,0);
    m->write_byte(0xff23,0xbf);
    m->write_byte(0xff24,0x77);
    m->write_byte(0xff25,0xf3);
    m->write_byte(0xff26,0xf1);
    m->write_byte(0xff40,0x91);
    m->write_byte(0xff42,0);
    m->write_byte(0xff43,0);
    m->write_byte(0xff45,0);
    m->write_byte(0xff47,0xfc);
    m->write_byte(0xff48,0xff);
    m->write_byte(0xff49,0xff);
    m->write_byte(0xff4a,0);
    m->write_byte(0xff4b,0);
    m->write_byte(0xffff,0);
}

int cpu::calc_frame() {
    if(frame_cycles>=70224) {
        frame_cycles-=70224;
    }
    else if(frame_cycles!=0) {
        printf("Why did I start this %d cycles into a frame?\n",frame_cycles);
        frame_cycles=0;
    }
    int retval=0;
    unsigned char opb1=0;
    unsigned char opb2=0;
    char opsb1=0;
    unsigned short opw1=0;
    while(frame_cycles<70224&&retval>=0) {
        unsigned char opcode=m->read_byte(*pc);
        printf("%04x: %02x\t",*pc,opcode);
        switch (opcode) {
            case 0x00:
                printf("NOP\n");
                retval+=4;
                frame_cycles+=4;
                total_cycles+=4;
                (*pc)++;
                break;
            case 0x31:
                opw1=m->read_word((*pc)+1);
                printf("LD SP,%04x\n",opw1);
                (*sp)=opw1;
                retval+=12;
                frame_cycles+=12;
                total_cycles+=12;
                (*pc)+=3;
                break;
            case 0x38:
                opsb1=char(m->read_byte((*pc)+1));
                if(opsb1<0) {
                    printf("JR C, -%02x\n",(~opsb1)+1);
                } else {
                    printf("JR C, %02x\n",opsb1);
                }
                (*pc)+=2;
                if(((*f)&CARRY_FLAG)>0) {
                    (*pc)=(opsb1+char(*pc));
                }
                retval+=12;
                frame_cycles+=12;
                total_cycles+=12;
                break;                    
            case 0x3e:
                opb1=m->read_byte((*pc)+1);
                printf("LD A,%02x\n",opb1);
                (*a)=opb1;
                retval+=8;
                frame_cycles+=8;
                total_cycles+=8;
                (*pc)+=2;
                break;
            case 0x91:
                printf("SUB C\n");
                if(*a>=*c) {
                    (*f)|=CARRY_FLAG;
                }
                else {
                    (*f)&=(~CARRY_FLAG);
                }
                if(((*a)&0xf)>=((*c)&0xf)) {
                    (*f)|=HALF_FLAG;
                }
                else {
                    (*f)&=(~HALF_FLAG);
                }
                (*a)-=(*c);
                set_neg(true);
                set_zero(*a);
                (*pc)++;
                retval+=4;
                frame_cycles+=4;
                total_cycles+=4;
                break;
            case 0xc3:
                opw1=m->read_word((*pc)+1);
                printf("JP %04x\n",opw1);
                (*pc)=opw1;
                retval+=12;
                frame_cycles+=12;
                total_cycles+=12;
                break;
            case 0xe0:
                opb1=m->read_byte((*pc)+1);
                printf ("LD ($FF00+%02x), A\n",opb1);
                m->write_byte(0xff00+opb1,(*a));
                retval+=12;
                frame_cycles+=12;
                total_cycles+=12;
                (*pc)+=2;
                break;
            case 0xea:
                opw1=m->read_word((*pc)+1);
                printf ("LD (%04x), A\n",opw1);
                m->write_byte(opw1,(*a));
                retval+=16;
                frame_cycles+=16;
                total_cycles+=16;
                (*pc)+=3;
                break;
            case 0xf0:
                opb1=m->read_byte((*pc)+1);
                printf ("LD A, ($FF00+%02x)\n",opb1);
                *a=m->read_byte(0xff00+opb1);
                retval+=12;
                frame_cycles+=12;
                total_cycles+=12;
                (*pc)+=2;
                break;
            case 0xf3:
                printf("DI\n");
                int_enable=false;
                retval+=4;
                frame_cycles+=4;
                total_cycles+=4;
                (*pc)++;
                break;               
            case 0xfe:
                opb1=m->read_byte((*pc)+1);
                printf("CP $%02x (A(%02x)-%02x)\n",opb1,(*a),opb1);
                printf("%02x is %02x-%02x.\n",((*a)-opb1)&0xff,(*a),opb1);
                set_zero((*a)-opb1);
                set_neg(true);
                if(((*a)&0xf)>=((*c)&0xf)) {
                    (*f)|=HALF_FLAG;
                    printf("Setting half\n");
                }
                else {
                    printf("Clearing half\n");
                    (*f)&=(~HALF_FLAG);
                }
                if((*a)>=opb1) {
                    printf("Setting carry\n");
                    (*f)|=CARRY_FLAG;
                }
                else {
                    printf("Clearing carry\n");
                    (*f)&=(~CARRY_FLAG);
                }
                retval+=8;
                frame_cycles+=4;
                total_cycles+=4;
                (*pc)+=2;
                break;
            default:
                printf("I don't recognize opcode $%02x. Aborting!\n",opcode);
                retval=-1;
                break;
        }
        printf("\n\n");
    }
}

void cpu::set_zero(unsigned char arg) {
    if(arg==0)
        (*f)|=ZERO_FLAG;
    else
        (*f)&=(~ZERO_FLAG);
}
void cpu::set_neg(bool arg) {
    if(arg)
        (*f)|=NEG_FLAG;
    else
        (*f)&=(~NEG_FLAG);
}
void cpu::set_carry(unsigned char arg1,unsigned char arg2) {
    if((int(arg1)+int(arg2))>0xff)
        (*f)|=CARRY_FLAG;
    else
        (*f)&=(~CARRY_FLAG);
}
void cpu::set_half(unsigned char arg) {
    if(arg>0xf)
        (*f)|=HALF_FLAG;
    else
        (*f)&=(~HALF_FLAG);
}
